package Physics;

import java.util.Vector;

public class ContactResolver {
	//////////////////////////
	// private member variables
	//////////////////////////
	/**
	 * The max number of iterations to use when
	 * resolving velocity
	 */
	int m_velocityIterations;
	
	/**
	 * The max number of iterations to use when
	 * resolving position
	 */
	int m_positionIterations;
	
	/**
	 * To avoid instability. Values smaller than this
	 * are considered to be zero.
	 */
	float m_velocityEpsilon = 0.01f;
	
	/**
	 * To avoid instability. Values smaller than this
	 * are considered zero.
	 */
	float m_positionEpsilon = 0.01f;
	
	/**
	 * Are the internal values valid?
	 */
	boolean validSettings;
	
	
	//////////////////////////
	// public member variables
	//////////////////////////
	/**
	 * Number of velocity iterations used so far
	 */
	public int m_velocityIterationsUsed;
	
	/**
	 * Number of position iterations used so far
	 */
	public int m_positionIterationsUsed;
	
	//////////////////////////
	// private member functions
	//////////////////////////
	/**
	 * Prepares the contacts for processing this iteration. Makes
	 * sure the internal data is configured correctly, and the 
	 * objects are awake.
	 */
	void prepareContacts(Vector<Contact> contactArray, float dt)
	{
		// Generate contact velocity and axis information.
	    for (int i = 0; i < contactArray.size(); ++i)
	    {
	        // Calculate the internal contact data (inertia, basis, etc).
	        contactArray.get(i).calculateInternals(dt);
	    }
	}
	
	/**
	 * Resolves velocity events
	 */
	void adjustVelocities(Vector<Contact> contactArray, float dt)
	{
		Vector3 [] velocityChange = new Vector3[2];
		Vector3 [] rotationChange = new Vector3[2];
		
		for (int i = 0; i < 2; ++i)
		{
			velocityChange[i] = new Vector3(0.0f, 0.0f, 0.0f);
			rotationChange[i] = new Vector3(0.0f, 0.0f, 0.0f);
		}
	    Vector3 deltaVel;

	    // iteratively handle impacts in order of severity.
	    m_velocityIterationsUsed = 0;
	    while (m_velocityIterationsUsed < m_velocityIterations)
	    {
	        // Find contact with maximum magnitude of probable velocity change.
	        float max = m_velocityEpsilon;
	        int index = contactArray.size();
	        for (int i = 0; i < contactArray.size(); i++)
	        {
	            if (contactArray.get(i).m_desiredDeltaVelocity > max)
	            {
	                max = contactArray.get(i).m_desiredDeltaVelocity;
	                index = i;
	            }
	        }
	        if (index == contactArray.size()) break;

	        // Match the awake state at the contact
	        contactArray.get(index).matchAwakeState();

	        // Do the resolution on the contact that came out top.
	        contactArray.get(index).applyVelocityChange(velocityChange, rotationChange);

	        // With the change in velocity of the two bodies, the update of
	        // contact velocities means that some of the relative closing
	        // velocities need recomputing.
	        for (int i = 0; i < contactArray.size(); i++)
	        {
	            // Check each body in the contact
	            for (int b = 0; b < 2; b++) if (contactArray.get(i).m_objects[b] != null)
	            {
	                // Check for a match with each body in the newly
	                // resolved contact
	                for (int d = 0; d < 2; d++)
	                {
	                    if (contactArray.get(i).m_objects[b] == contactArray.get(index).m_objects[d])
	                    {
	                        deltaVel = velocityChange[d].add(rotationChange[d].crossProduct(
	                            		contactArray.get(i).m_relativeContactPosition[b]));

	                        // The sign of the change is negative if we're dealing
	                        // with the second body in a contact.
	                        Vector3 transpose = contactArray.get(i).m_contactToWorld.transformTranspose(deltaVel);
	                        transpose.multiplyAndSet(b!=0?-1.0f:1.0f);
	                        contactArray.get(i).m_contactVelocity.addAndSet(transpose);
	                        //contactArray.get(i).m_contactVelocity.addAndSet(contactArray.get(i).m_contactToWorld.transformTranspose(deltaVel).multiply((b != 0?-1.0f:1.0f)));
	                        contactArray.get(i).calculateDesiredDeltaVelocity(dt);
	                    }
	                }
	            }
	        }
	        m_velocityIterationsUsed++;
	    }
	}
	
	/**
	 * Resolves positional events
	 */
	void adjustPositions(Vector<Contact> contactArray, float dt)
	{
		int i,index;
	    Vector3 [] linearChange = new Vector3[2];
	    Vector3 [] angularChange = new Vector3[2];
	    
	    for(int x = 0; x < 2; x++)
	    {
	    	linearChange[x] = new Vector3(0.0f, 0.0f, 0.0f);
	    	angularChange[x] = new Vector3(0.0f, 0.0f, 0.0f);
	    }
	    
	    float max;
	    Vector3 deltaPosition;

	    // iteratively resolve interpenetrations in order of severity.
	    m_positionIterationsUsed = 0;
	    while (m_positionIterationsUsed < m_positionIterations)
	    {
	        // Find biggest penetration
	        max = m_positionEpsilon;
	        index = contactArray.size();
	        for (i=0; i<contactArray.size(); i++)
	        {
	            if (contactArray.get(i).m_penetration > max)
	            {
	                max = contactArray.get(i).m_penetration;
	                index = i;
	            }
	        }
	        if (index == contactArray.size()) break;

	        // Match the awake state at the contact
	        contactArray.get(index).matchAwakeState();

	        // Resolve the penetration.
	        contactArray.get(index).applyPositionChange(
	            linearChange,
	            angularChange,
	            max);

	        // Again this action may have changed the penetration of other
	        // bodies, so we update contacts.
	        for (i = 0; i < contactArray.size(); i++)
	        {
	            // Check each body in the contact
	            for (int b = 0; b < 2; b++) if (contactArray.get(i).m_objects[b] != null)
	            {
	                // Check for a match with each body in the newly
	                // resolved contact
	                for (int d = 0; d < 2; d++)
	                {
	                    if (contactArray.get(i).m_objects[b] == contactArray.get(index).m_objects[d])
	                    {
	                        deltaPosition = linearChange[d].add(angularChange[d].crossProduct(
	                            		contactArray.get(i).m_relativeContactPosition[b]));

	                        // The sign of the change is positive if we're
	                        // dealing with the second body in a contact
	                        // and negative otherwise (because we're
	                        // subtracting the resolution)..
	                        contactArray.get(i).m_penetration +=
	                            deltaPosition.dotProduct(contactArray.get(i).m_contactNormal)
	                            * (b != 0?1.0f:-1.0f);
	                    }
	                }
	            }
	        }
	        m_positionIterationsUsed++;
	    }
	}
	
	//////////////////////////
	// public member functions
	//////////////////////////
	/**
	 * Resolves a set of contacts for penetration and velocity
	 */
	public void resolveContacts(Vector<Contact> contactArray, float dt)
	{
		// Make sure we have something to do.
	    if (contactArray.size() == 0) return;
	    if (!isValid()) return;

	    // Prepare the contacts for processing
	    prepareContacts(contactArray, dt);

	    // Resolve the interpenetration problems with the contacts.
	    adjustPositions(contactArray, dt);

	    // Resolve the velocity problems with the contacts.
	    adjustVelocities(contactArray, dt);
	    
	    float x = 0.0f;
	}
	
	/**
	 * Constructor.
	 * Sets the number of iterations (both the same) and epsilon values.
	 */
	public ContactResolver(int iterations, float velocityEpsilon, float positionEpsilon)
	{
		setIterations(iterations, iterations);
	    setEpsilon(velocityEpsilon, positionEpsilon);
	}
	
	/**
	 * Constructor.
	 * Sets the number of iterations (independently) and
	 * epsilon values.
	 */
	public ContactResolver(int velocityIterations, int positionIterations, float velocityEpsilon, float positionEpsilon)
	{
		setIterations(velocityIterations, positionIterations);
	    setEpsilon(velocityEpsilon, positionEpsilon);
	}
	
	/**
	 * Are the internal values valid?
	 */
	public boolean isValid()
	{
		return (m_velocityIterations > 0 &&
				m_positionIterations > 0 &&
				m_positionEpsilon > 0.0f &&
				m_velocityEpsilon > 0.0f);
	}
	
	/**
	 * Set the number of iterations per stage (independently)
	 */
	public void setIterations(int velocityIterations, int positionIterations)
	{
		m_velocityIterations = velocityIterations;
	    m_positionIterations = positionIterations;
	}
	
	/**
	 * Set the number of iterations per stage (both the same)
	 */
	public void setIterations(int iterations)
	{
		setIterations(iterations, iterations);
	}
	
	/**
	 * Set tolerance values
	 */
	public void setEpsilon(float velocityEpsilon, float positionEpsilon)
	{
		m_velocityEpsilon = velocityEpsilon;
	    m_positionEpsilon = positionEpsilon;
	}
	
	
	
}
